
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       auto_yaw.c
  * @author     baiyang
  * @date       2022-3-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "auto_yaw.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// roi_yaw - returns heading towards location held in roi
float autoyaw_roi_yaw(AutoYaw * autoyaw)
{
    return vec3_get_bearing_cd(&fms.ahrs->relpos_cm, &autoyaw->roi);
}

float autoyaw_look_ahead_yaw(AutoYaw * autoyaw)
{
    const float speed_sq = vec3_length_squared_xy(&fms.ahrs->velocity_cm);
    // Commanded Yaw to automatically look ahead.
    if (fms_position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
        autoyaw->_look_ahead_yaw = degrees(atan2f(fms.ahrs->velocity_cm.y,fms.ahrs->velocity_cm.x))*100.0f;
    }
    return autoyaw->_look_ahead_yaw;
}

void autoyaw_set_mode_to_default(AutoYaw * autoyaw, bool rtl)
{
    autoyaw_set_mode(autoyaw, autoyaw_default_mode(rtl));
}

// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
enum autopilot_yaw_mode autoyaw_default_mode(bool rtl)
{
    switch (fms.g.wp_yaw_behavior) {

    case WP_YAW_BEHAVIOR_NONE:
        return AUTO_YAW_HOLD;

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
        if (rtl) {
            return AUTO_YAW_HOLD;
        } else {
            return AUTO_YAW_LOOK_AT_NEXT_WP;
        }

    case WP_YAW_BEHAVIOR_LOOK_AHEAD:
        return AUTO_YAW_LOOK_AHEAD;

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
    default:
        return AUTO_YAW_LOOK_AT_NEXT_WP;
    }
}

// set_mode - sets the yaw mode for auto
void autoyaw_set_mode(AutoYaw * autoyaw, enum autopilot_yaw_mode yaw_mode)
{
    // return immediately if no change
    if (autoyaw->_mode == yaw_mode) {
        return;
    }
    autoyaw->_mode = yaw_mode;

    // perform initialisation
    switch (autoyaw->_mode) {

    case AUTO_YAW_HOLD:
        break;

    case AUTO_YAW_LOOK_AT_NEXT_WP:
        // wpnav will initialise heading when wpnav's set_destination method is called
        break;

    case AUTO_YAW_ROI:
        // look ahead until we know otherwise
        break;

    case AUTO_YAW_FIXED:
        // keep heading pointing in the direction held in fixed_yaw
        // caller should set the fixed_yaw
        break;

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        autoyaw->_look_ahead_yaw = fms.ahrs->yaw_sensor_cd;
        break;

    case AUTO_YAW_RESETTOARMEDYAW:
        // initial_armed_bearing will be set during arming so no init required
        break;

    case AUTO_YAW_ANGLE_RATE:
        break;

    case AUTO_YAW_RATE:
        // initialise target yaw rate to zero
        autoyaw->_yaw_rate_cds = 0.0f;
        break;

    case AUTO_YAW_CIRCLE:
        // no initialisation required
        break;
    }
}

// set_fixed_yaw - sets the yaw look at heading for auto mode
void autoyaw_set_fixed_yaw(AutoYaw * autoyaw, float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle)
{
    autoyaw->_last_update_ms = time_millis();

    autoyaw->_yaw_angle_cd = attctrl_get_att_target_euler_cd(fms.attitude_control).z;
    autoyaw->_yaw_rate_cds = 0.0f;

    // calculate final angle as relative to vehicle heading or absolute
    if (relative_angle) {
        autoyaw->_fixed_yaw_offset_cd = angle_deg * 100.0f * (direction >= 0 ? 1.0f : -1.0f);
    } else {
        // absolute angle
        autoyaw->_fixed_yaw_offset_cd = math_wrap_180_cd(angle_deg * 100.0 - autoyaw->_yaw_angle_cd);
        if ( direction < 0 && math_flt_positive(autoyaw->_fixed_yaw_offset_cd) ) {
            autoyaw->_fixed_yaw_offset_cd -= 36000.0f;
        } else if ( direction > 0 && math_flt_negative(autoyaw->_fixed_yaw_offset_cd) ) {
            autoyaw->_fixed_yaw_offset_cd += 36000.0f;
        }
    }

    // get turn speed
    if (!math_flt_positive(turn_rate_ds)) {
        // default to default slew rate
        autoyaw->_fixed_yaw_slewrate_cds = attctrl_get_slew_yaw_cds(fms.attitude_control);
    } else {
        autoyaw->_fixed_yaw_slewrate_cds = MIN(attctrl_get_slew_yaw_cds(fms.attitude_control), turn_rate_ds * 100.0f);
    }

    // set yaw mode
    autoyaw_set_mode(autoyaw, AUTO_YAW_FIXED);
}

// set_fixed_yaw - sets the yaw look at heading for auto mode
void autoyaw_set_yaw_angle_rate(AutoYaw * autoyaw, float yaw_angle_d, float yaw_rate_ds)
{
    autoyaw->_last_update_ms = time_millis();

    autoyaw->_yaw_angle_cd = yaw_angle_d * 100.0;
    autoyaw->_yaw_rate_cds = yaw_rate_ds * 100.0;

    // set yaw mode
    autoyaw_set_mode(autoyaw, AUTO_YAW_ANGLE_RATE);
}

// set_roi - sets the yaw to look at roi for auto mode
void autoyaw_set_roi(AutoYaw * autoyaw, const Location* roi_location)
{
    // if location is zero lat, lon and altitude turn off ROI
    if (roi_location->alt == 0 && roi_location->lat == 0 && roi_location->lng == 0) {
        // set auto yaw mode back to default assuming the active command is a waypoint command.  A more sophisticated method is required to ensure we return to the proper yaw control for the active command
        autoyaw_set_mode_to_default(autoyaw, false);
#if HAL_MOUNT_ENABLED
        // switch off the camera tracking if enabled
#endif  // HAL_MOUNT_ENABLED
    } else {
#if HAL_MOUNT_ENABLED
        // check if mount type requires us to rotate the quad

        // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink.  Currently we only handle mode 4 (see below)
        //      0: do nothing
        //      1: point at next waypoint
        //      2: point at a waypoint taken from WP# parameter (2nd parameter?)
        //      3: point at a location given by alt, lon, lat parameters
        //      4: point at a target given a target id (can't be implemented)
#else
        // if we have no camera mount aim the quad at the location
        if (location_get_vector_from_origin_neu(roi_location, &autoyaw->roi)) {
            autoyaw_set_mode(autoyaw, AUTO_YAW_ROI);
        }
#endif  // HAL_MOUNT_ENABLED
    }
}

// set auto yaw rate in centi-degrees per second
void autoyaw_set_rate(AutoYaw * autoyaw, float turn_rate_cds)
{
    autoyaw_set_mode(autoyaw, AUTO_YAW_RATE);
    autoyaw->_yaw_rate_cds = turn_rate_cds;
}

// yaw - returns target heading depending upon auto_yaw.mode()
float autoyaw_yaw(AutoYaw * autoyaw)
{
    switch (autoyaw->_mode) {

    case AUTO_YAW_ROI:
        // point towards a location held in roi
        return autoyaw_roi_yaw(autoyaw);

    case AUTO_YAW_FIXED: {
        // keep heading pointing in the direction held in fixed_yaw
        // with no pilot input allowed
        const uint32_t now_ms = time_millis();
        float dt = (now_ms - autoyaw->_last_update_ms) * 0.001f;
        autoyaw->_last_update_ms = now_ms;
        float yaw_angle_step = math_constrain_float(autoyaw->_fixed_yaw_offset_cd, - dt * autoyaw->_fixed_yaw_slewrate_cds, dt * autoyaw->_fixed_yaw_slewrate_cds);
        autoyaw->_fixed_yaw_offset_cd -= yaw_angle_step;
        autoyaw->_yaw_angle_cd += yaw_angle_step;
        return autoyaw->_yaw_angle_cd;
    }

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        return autoyaw_look_ahead_yaw(autoyaw);

    case AUTO_YAW_RESETTOARMEDYAW:
        // changes yaw to be same as when quad was armed
        return fms.initial_armed_bearing;

    case AUTO_YAW_CIRCLE:
#if MODE_CIRCLE_ENABLED

#endif
        // return the current attitude target
        return math_wrap_360_cd(attctrl_get_att_target_euler_cd(fms.attitude_control).z);

    case AUTO_YAW_ANGLE_RATE:{
        const uint32_t now_ms = time_millis();
        float dt = (now_ms - autoyaw->_last_update_ms) * 0.001;
        autoyaw->_last_update_ms = now_ms;
        autoyaw->_yaw_angle_cd += autoyaw->_yaw_rate_cds * dt;
        return autoyaw->_yaw_angle_cd;
    }

    case AUTO_YAW_LOOK_AT_NEXT_WP:
    default:
        // point towards next waypoint.
        // we don't use wp_bearing because we don't want the copter to turn too much during flight
        return posctrl_get_yaw_cd(fms.pos_control);
    }
}

// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float autoyaw_rate_cds(AutoYaw * autoyaw)
{
    switch (autoyaw->_mode) {

    case AUTO_YAW_HOLD:
    case AUTO_YAW_ROI:
    case AUTO_YAW_FIXED:
    case AUTO_YAW_LOOK_AHEAD:
    case AUTO_YAW_RESETTOARMEDYAW:
    case AUTO_YAW_CIRCLE:
        return 0.0f;

    case AUTO_YAW_ANGLE_RATE:
    case AUTO_YAW_RATE:
        return autoyaw->_yaw_rate_cds;

    case AUTO_YAW_LOOK_AT_NEXT_WP:
        return posctrl_get_yaw_rate_cds(fms.pos_control);
    }

    // return zero turn rate (this should never happen)
    return 0.0f;
}

/*------------------------------------test------------------------------------*/


